﻿#include "CvCalibCommon.h"

#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>

#include <iostream>

using namespace cv;
using namespace std;

bool CvCalibCommon::findChessboardCorners(cv::Mat &image, cv::Size &boardSize, std::vector<cv::Point2f> &corners)
{
    bool found = cv::findChessboardCorners(image, boardSize, corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE+
          CALIB_CB_FAST_CHECK );

    if(found) {
        Mat imageGray;
        if (image.channels() != 1) {
            cvtColor(image, imageGray, COLOR_BGR2GRAY);
        } else {
            imageGray = image;
        }
        /* 亚像素精确化 */
        cv::cornerSubPix(imageGray, corners, Size(5, 5), Size(-1, -1),
                      TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 100, 0.0001));
    }

    return found;
}

//! [compute_errors]
double CvCalibCommon::computeReprojectionErrors(
        const vector<vector<Point3f> >& objectPoints,
        const vector<vector<Point2f> >& imagePoints,
        const vector<Mat>& rvecs, const vector<Mat>& tvecs,
        const Mat& cameraMatrix, const Mat& distCoeffs,
        vector<float>& perViewErrors )
{
    vector<Point2f> imagePoints2;
    int i, totalPoints = 0;
    double totalErr = 0, err;
    perViewErrors.resize(objectPoints.size());

    for( i = 0; i < (int)objectPoints.size(); i++ )
    {
        projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i],
                      cameraMatrix, distCoeffs, imagePoints2);
        err = norm(Mat(imagePoints[i]), Mat(imagePoints2), NORM_L2);
        int n = (int)objectPoints[i].size();
        perViewErrors[i] = (float)std::sqrt(err*err/n);
        totalErr += err*err;
        totalPoints += n;
    }

    return std::sqrt(totalErr/totalPoints);
}

Point2f CvCalibCommon::uvToDistortedUV(Point2f &uv, Mat &intrinsic, Mat &intrinsicInvert, Mat &distCoeffs)
{
    Mat point(3, 1, CV_64F);
    point.at<double>(0,0) = uv.x;
    point.at<double>(1,0) = uv.y;
    point.at<double>(2,0) = 1;

    Mat c = intrinsicInvert * point;
    double x = c.at<double>(0,0)/c.at<double>(2,0);
    double y = c.at<double>(1,0)/c.at<double>(2,0);

    const double* const K = distCoeffs.ptr<double>();
    double k1 = K[0];
    double k2 = K[1];
    double k3 = K[4];
    double p1 = K[2];
    double p2 = K[3];

    double r2=x*x + y*y;
    double r4 = r2*r2;
    double r6 = r4*r2;
    double a1 = 2*x*y;
    double a2 = r2 + 2*x*x;
    double a3 = r2 + 2*y*y;
    double cdist = 1 + k1*r2 + k2*r4 + k3*r6;
    double xd0 = x*cdist + p1 * a1 + p2*a2;
    double yd0 = y*cdist + p1 * a3 + p2*a1;

    Point2f uv2;
    uv2.x = (intrinsic.at<double>(0,0)*xd0 + intrinsic.at<double>(0, 1)*yd0 + intrinsic.at<double>(0, 2)*1)/1;
    uv2.y = (intrinsic.at<double>(1,0)*xd0 + intrinsic.at<double>(1, 1)*yd0 + intrinsic.at<double>(1, 2)*1)/1;

    return uv2;
}

Point2f CvCalibCommon::xyz2uv(Point3f worldPoint, float intrinsic[3][3], float translation[1][3],
    float rotation[3][3], double distCoeffs[5])
{
    //    [fx s x0]							[Xc]		[Xw]		[u]	  1		[Xc]
    //K = |0 fy y0|       TEMP = [R T]		|Yc| = TEMP*|Yw|		| | = —*K *|Yc|
    //    [ 0 0 1 ]							[Zc]		|Zw|		[v]	  Zc	[Zc]
    //													[1 ]
    Point3f c;
    c.x = rotation[0][0]*worldPoint.x + rotation[0][1]*worldPoint.y + rotation[0][2]*worldPoint.z + translation[0][0]*1;
    c.y = rotation[1][0]*worldPoint.x + rotation[1][1]*worldPoint.y + rotation[1][2]*worldPoint.z + translation[0][1]*1;
    c.z = rotation[2][0]*worldPoint.x + rotation[2][1]*worldPoint.y + rotation[2][2]*worldPoint.z + translation[0][2]*1;

    Point2f uv;
    if (0) {
        uv.x = (intrinsic[0][0]*c.x + intrinsic[0][1]*c.y + intrinsic[0][2]*c.z)/c.z;
        uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z;
    } else {
        double x = c.x/c.z;
        double y = c.y/c.z;

        double k1 = distCoeffs[0];
        double k2 = distCoeffs[1];
        double k3 = distCoeffs[4];
        double p1 = distCoeffs[2];
        double p2 = distCoeffs[3];

        double r2=x*x + y*y;
        double r4 = r2*r2;
        double r6 = r4*r2;
        double a1 = 2*x*y;
        double a2 = r2 + 2*x*x;
        double a3 = r2 + 2*y*y;
        double cdist = 1 + k1*r2 + k2*r4 + k3*r6;
        double xd0 = x*cdist + p1 * a1 + p2*a2;
        double yd0 = y*cdist + p1 * a3 + p2*a1;

        uv.x = (intrinsic[0][0]*xd0 + intrinsic[0][1]*yd0 + intrinsic[0][2]*1)/1;
        uv.y = (intrinsic[1][0]*xd0 + intrinsic[1][1]*yd0 + intrinsic[1][2]*1)/1;
    }

    return uv;
}

Point2f CvCalibCommon::xyz2uv(Point3f &worldPoint, Mat &intrinsic, Mat &R, Mat &T, Mat &distCoeffs, bool isDistorted)
{
    //    [fx s x0]							[Xc]		[Xw]		[u]	  1		[Xc]
     //K = |0 fy y0|       TEMP = [R T]		|Yc| = TEMP*|Yw|		| | = —*K *|Yc|
     //    [ 0 0 1 ]							[Zc]		|Zw|		[v]	  Zc	[Zc]
     //
     Mat pointW = Mat(3, 1, CV_64FC1);
     // pointW.reshape(3,1);
     pointW.at<double>(0,0) = worldPoint.x;
     pointW.at<double>(1,0) = worldPoint.y;
     pointW.at<double>(2,0) = worldPoint.z;
     // cout << "pointW" << pointW << endl;
//     cout << "R" << R << endl;
//     cout << "T" << T << endl;
     Mat pointC = R * pointW + T;

     Point3f c(pointC.at<double>(0,0), pointC.at<double>(1,0), pointC.at<double>(2,0));
     Point2f uv;

     double *intrinsicData = intrinsic.ptr<double>(0);
     if (isDistorted){
         const double* const K = distCoeffs.ptr<double>();
         double k1 = K[0];
         double k2 = K[1];
         double k3 = K[4];
         double p1 = K[2];
         double p2 = K[3];
         double x = c.x/c.z;
         double y = c.y/c.z;

         double r2=x*x + y*y;
         double r4 = r2*r2;
         double r6 = r4*r2;
         double a1 = 2*x*y;
         double a2 = r2 + 2*x*x;
         double a3 = r2 + 2*y*y;
         double cdist = 1 + k1*r2 + k2*r4 + k3*r6;
         double xd0 = x*cdist + p1 * a1 + p2*a2;
         double yd0 = y*cdist + p1 * a3 + p2*a1;

         uv.x = (intrinsicData[0]*xd0 + intrinsicData[1]*yd0 + intrinsicData[2]*1);
         intrinsicData = intrinsic.ptr<double>(1);
         uv.y = (intrinsicData[0]*xd0 + intrinsicData[1]*yd0 + intrinsicData[2]*1);
     } else {
         uv.x = (intrinsicData[0]*c.x + intrinsicData[1]*c.y + intrinsicData[2]*c.z)/c.z;
         intrinsicData = intrinsic.ptr<double>(1);
         uv.y = (intrinsicData[0]*c.x + intrinsicData[1]*c.y + intrinsicData[2]*c.z)/c.z;
     }

     return uv;
}

Point3f CvCalibCommon::uv2xyz(Point2f &uvLeft, Point2f &uvRight,
                              Mat &leftIntrinsic, Mat &leftRotation, Mat &leftTranslation,
                              Mat &rightIntrinsic, Mat &rightRotation, Mat &rightTranslation)
{
    //  [u1]      |X|					  [u2]      |X|
    //Z*|v1| = Ml*|Y|					Z*|v2| = Mr*|Y|
    //  [ 1]      |Z|					  [ 1]      |Z|
    //			  |1|								|1|
    Mat mLeftRT = Mat(3,4,CV_64F);//左相机M矩阵
    hconcat(leftRotation,leftTranslation,mLeftRT);
    Mat mLeftM = leftIntrinsic * mLeftRT;
    //cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;

    Mat mRightRT = Mat(3,4,CV_64F);//右相机M矩阵
    hconcat(rightRotation,rightTranslation,mRightRT);
    Mat mRightM = rightIntrinsic * mRightRT;
    //cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;

    //最小二乘法A矩阵
    Mat A = Mat(4,3,CV_64F);
    A.at<double>(0,0) = uvLeft.x * mLeftM.at<double>(2,0) - mLeftM.at<double>(0,0);
    A.at<double>(0,1) = uvLeft.x * mLeftM.at<double>(2,1) - mLeftM.at<double>(0,1);
    A.at<double>(0,2) = uvLeft.x * mLeftM.at<double>(2,2) - mLeftM.at<double>(0,2);

    A.at<double>(1,0) = uvLeft.y * mLeftM.at<double>(2,0) - mLeftM.at<double>(1,0);
    A.at<double>(1,1) = uvLeft.y * mLeftM.at<double>(2,1) - mLeftM.at<double>(1,1);
    A.at<double>(1,2) = uvLeft.y * mLeftM.at<double>(2,2) - mLeftM.at<double>(1,2);

    A.at<double>(2,0) = uvRight.x * mRightM.at<double>(2,0) - mRightM.at<double>(0,0);
    A.at<double>(2,1) = uvRight.x * mRightM.at<double>(2,1) - mRightM.at<double>(0,1);
    A.at<double>(2,2) = uvRight.x * mRightM.at<double>(2,2) - mRightM.at<double>(0,2);

    A.at<double>(3,0) = uvRight.y * mRightM.at<double>(2,0) - mRightM.at<double>(1,0);
    A.at<double>(3,1) = uvRight.y * mRightM.at<double>(2,1) - mRightM.at<double>(1,1);
    A.at<double>(3,2) = uvRight.y * mRightM.at<double>(2,2) - mRightM.at<double>(1,2);

    //最小二乘法B矩阵
    Mat B = Mat(4,1,CV_64F);
    B.at<double>(0,0) = mLeftM.at<double>(0,3) - uvLeft.x * mLeftM.at<double>(2,3);
    B.at<double>(1,0) = mLeftM.at<double>(1,3) - uvLeft.y * mLeftM.at<double>(2,3);
    B.at<double>(2,0) = mRightM.at<double>(0,3) - uvRight.x * mRightM.at<double>(2,3);
    B.at<double>(3,0) = mRightM.at<double>(1,3) - uvRight.y * mRightM.at<double>(2,3);

    Mat XYZ = Mat(3,1,CV_64F);
    //采用SVD最小二乘法求解XYZ
    solve(A,B,XYZ,DECOMP_SVD);

    //cout<<"空间坐标为 = "<<endl<<XYZ<<endl;

    // 世界坐标系中坐标
    Point3f world;
    world.x = XYZ.at<double>(0,0);
    world.y = XYZ.at<double>(1,0);
    world.z = XYZ.at<double>(2,0);

    return world;
}

void CvCalibCommon::findLightBalls(Mat &grayImage, int nThresh, std::vector<Point2f> &outPoints, const float rMin)
{
    Mat threshold_output;
    vector<vector<Point> >  contours;
    vector<Vec4i> hierarchy;
    // blur
    blur(grayImage,grayImage,Size(3,3));

    // Canny

    // threshold
    threshold(grayImage,threshold_output,nThresh,255,THRESH_BINARY);

    // contours
    findContours(threshold_output,contours,hierarchy,RETR_TREE,CHAIN_APPROX_SIMPLE,Point(0,0));

    vector<vector<Point> > contours_poly(contours.size());
    vector<Point2f> center(contours.size());
    vector<float>radius(contours.size());

    for (uint i = 0;i < contours.size();i++){
        approxPolyDP(Mat(contours[i]),contours_poly[i],3,true);
        minEnclosingCircle(contours_poly[i],center[i],radius[i]);
        if(radius[i] >= rMin) {
            outPoints.push_back(center[i]);
        }
    }
}
